#!/usr/bin/env python
#coding=utf-8

import importlib
import os

import rospy
from sensor_msgs.msg import Joy
import time
from geometry_msgs.msg import Twist, Vector3
from std_msgs.msg import String as StringMsg
from rocr6_msgs.msg import Goal,Gripper,xJoint,Feedback
from zeus_s2_msgs.msg import joy_control

class JoyTeleop:
	def __init__(self):
		self.active = 0
		self.flag = 0
		self.count = 0
		self.off_flag=0
		self.x_speed_scale = rospy.get_param('~x_speed_scale')
		self.w_speed_scale = rospy.get_param('~w_speed_scale')

		self.speed_x_vol_  = rospy.get_param('~speed_x_vol_',0.1)
		self.speed_x_time_ = rospy.get_param('~speed_x_time_',0.02)
		self.speed_w_vol_ = rospy.get_param('~speed_w_vol_',0.2)
		self.speed_w_time_ = rospy.get_param('~speed_w_time_',0.02)


		self.velocity = Twist()
		self.speed_planning = Twist()
		self.rocr6 = Goal()
		self.jiazhua = Gripper()
		self.joyps3 = joy_control()
		self.rate = rospy.Rate(20)

		#speed planning
		self.current_time_ = 0
		self.last_time_ = 0
		self.save_time_= 0
		self.current_speed_ = 0
		self.recv_speed_= 0
		self.save_time_ = 0
		self.last_time_ = time.time()
		self.speed_x_ = 0
		self.speed_w_ = 0
		self.last_time_x_ = 0
		self.last_time_w_ = 0
		self.active_time_ = 0
		self.speed_vol_ = 0
		self.speed_time_vol_ = 0
		

        
		# self.cmdVelPublisher = rospy.Publisher('/cmd_moment', joy_control, queue_size = 3)
		self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size = 3)
		self.joySubscriber = rospy.Subscriber('joy', Joy, self.buttonCallback)
		self.rocr6Publisher = rospy.Publisher('/rocr6_msgs/goal', Goal, queue_size = 3)
		self.jiazhuaPublisher = rospy.Publisher('/rocr6_msgs/gripper', Gripper, queue_size = 3)
		self.loop()
	def buttonCallback(self, joy_data):
		# print "X : %f  ,Y : %f  ,Z: %f  ,joy_data.axes[0]: %f ,joy_data.axes[1]: %f  ,joy_data.axes[2]: %f  ,\
		# joy_data.axes[3]: %f ,joy_data.axes[4]: %f ,joy_data.axes[5]: %f,joy_data.buttons[4]:%f,joy_data.buttons[5]:%f" %(self.velocity.linear.x , self.velocity.linear.y,self.velocity.angular.z,joy_data.axes[0],\
		# joy_data.axes[1],joy_data.axes[2], joy_data.axes[3] , joy_data.axes[4], joy_data.axes[5],joy_data.buttons[4] ,joy_data.buttons[5])

		if(joy_data.buttons[4] == 1):
			self.velocity.linear.x = self.x_speed_scale * joy_data.axes[4]
			self.velocity.angular.z = self.w_speed_scale * joy_data.axes[0]		
			 
			self.active_time_ = time.time()
			self.active = 1
		else:
			self.velocity.linear = Vector3(0.,0.,0.)
			self.velocity.angular = Vector3(0.,0.,0.)
			self.speed_x_ = 0
			self.speed_w_=0
			self.recv_speed_ = 0
			self.current_speed_ = 0
			self.active = 0			
			self.cmdVelPublisher.publish(self.velocity)

		if(joy_data.axes[2] == -1 and joy_data.axes[5] == -1):
			self.off_flag = 1
		else:
			self.off_flag = 0

	def Remote_control_speed_planning(self):
		if(self.recv_speed_ > self.current_speed_):
			self.current_speed_ += self.speed_vol_          
			if(self.recv_speed_ <= self.current_speed_):		#判断是否过临界值
				self.current_speed_ = self.recv_speed_
				# print("recv_speed_:" ,self.recv_speed_ ,"current_speed:",self.current_speed_)
			# print("+++++",self.recv_speed_ , self.current_speed_)
		if(self.recv_speed_ < self.current_speed_):
			self.current_speed_ -=  self.speed_vol_    
			if(self.recv_speed_ >= self.current_speed_):		#判断是否过临界值
				self.current_speed_ = self.recv_speed_
				# print("recv_speed_:",self.recv_speed_ ,"current_speed:",self.current_speed_)
			# print("-------",self.recv_speed_ ,self.current_speed_) 

	def Detection_time(self,last_time_):
		# print(last_time_)
		if (time.time()-last_time_) >= self.speed_time_vol:
			# print("daoshijian")
			last_time_ = time.time()
			self.Remote_control_speed_planning()
			# print("时间 %f",time.time())
			# print(time.time)				
		return last_time_
		
                            
	def loop(self):
		while not rospy.is_shutdown():
			if(self.off_flag== 1):
				self.count += 1						
				if(self.count == 50):
					print " shijiandao!!!!!!!!!!!!!!!!"
					os.system("poweroff")
				else:
					print "count:%d " %(self.count)
			else:
				self.count = 0

			
			if(self.active == 1):
				self.recv_speed_ = self.velocity.linear.x
				self.current_speed_ = self.speed_x_
				# print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
				self.speed_vol_ = self.speed_x_vol_
				self.speed_time_vol = self.speed_x_time_
				self.last_time_x_ = self.Detection_time(self.last_time_x_)
				self.speed_x_ = self.current_speed_
				self.speed_planning.linear.x = self.speed_x_


				self.recv_speed_ = self.velocity.angular.z
				self.current_speed_ = self.speed_w_
				# print("wwwwwwwwwwwwwwwwww")
				self.speed_vol_ = self.speed_w_vol_
				self.speed_time_vol = self.speed_w_time_
				self.last_time_w_ =self. Detection_time(self.last_time_w_)
				self.speed_w_ = self.current_speed_
				self.speed_planning.angular.z = self.speed_w_

			 	self.cmdVelPublisher.publish(self.speed_planning)
				
			self.rate.sleep()
			

	
if __name__ == '__main__':
	rospy.init_node('joy_teleop')
	joy = JoyTeleop()
	try:
		rospy.spin()
	except	rospy.ROSInterruptException:
		print('exception')
			
	

